/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/

// -*- coding:iso-8859-15 mode: c++ -*-
/** 
 * \file
 * \brief MaCI_Ranging Player for logged files.
 * \author Matthieu Myrsky <matthieu.myrsky@tkk.fi>
 *
 * $Id: RangingFilePlayer.cpp,v 1.9 2009-11-16 16:07:38 morsko Exp $
 *
 */

#include "RangingFilePlayer.hpp"
#include <sstream>

RangingFilePlayer::RangingFilePlayer( MaCI::Ranging::CRangingServer *aRangingServer,
                                        std::string aFileName)
  :PlayerModule(),
   iRangingServer(aRangingServer),
   iData(),
   iTimes(),
   iBegin(0,0),
   iEnd(0,0),
   iFile(NULL),
   iName("")
{
 
  
  dPrint(3,"Opening file '%s'",aFileName.c_str());
  iFile= fopen(aFileName.c_str(),"rt");
  char c[200];
  char interfaceName[64];
  std::string temp;
  //Read the four first lines..and get the interfacename...
  fgets (c, 200,iFile );

  fscanf(iFile,"%s %s %s %s",c,c,c,interfaceName);
  iName = "MaCI_Ranging."+std::string(interfaceName);
  iRangingServer->SetInterfaceInstanceName(std::string(interfaceName));
  fgets (c, 200,iFile );
  fgets (c, 200,iFile );
  fgets (c, 200,iFile );
  dPrint(1,"%s",c);

  std::string additionalInformation(c);
  //Search for device limits
  std::string::size_type loc = additionalInformation.find( "maxDistance:", 0 );
  std::istringstream stream;
  std::string s(c);
  stream.str(s);
  if( loc != std::string::npos ) {

    float maxDistance,minDistance;
    stream >> temp >>maxDistance >>temp >>temp>> minDistance;
    iRangingServer->SetDeviceLimits(minDistance, maxDistance);
  }
  //Search for device position
  loc = additionalInformation.find( "positionX", 0 );
  if( loc != std::string::npos ) {
    float x=1,y=1.1,z=1.1,heading=1.1;
    stream >>temp>>x>>temp>>temp>>y>>temp>>temp>>z>>temp >>temp >>heading;
    dPrint(10,"got device position (%f, %f, %f, %f)",x,y,z,heading);
    iRangingServer->SetDevicePosition(x,y,z,heading);
  }

  ReadData();
  iRangingServer->Open();
}
//*****************************************************************************

RangingFilePlayer::~RangingFilePlayer()
{
  fclose(iFile);
  delete(iRangingServer);
}
//*****************************************************************************

bool RangingFilePlayer::ReadData()
{
  using namespace MaCI::Ranging;
  MaCI::Common::TTimestamp timestamp;
  float beam_width = 0.0,beam_count = 0.0;
  std::vector<float> angles;
  std::vector<float> distances;
  float angle = 0.0,distance = 0.0;
  int number = 1;
  bool first = true;
  while(number != EOF){
  
    angles.clear();
    distances.clear();
    number = 1;
    number = fscanf(iFile,"%u %u %f %f",
                    &(timestamp.timestamp_s),
                    &(timestamp.timestamp_us),
                    &beam_width,
                    &beam_count);
   
    if(first && number == 4){
      iBegin = timestamp.GetGimTime();
      first = false;
    }
    //    dPrint(1,"timestamp %s, number %d",timestamp.GetGimTime().toString().c_str(),number);

    for(unsigned j = 0; j< beam_count;j++){
      number = fscanf(iFile,"%f %f",
                      &angle,
                      &distance);

      angles.push_back(angle);
      distances.push_back(distance);

    }

    if(angles.size() >0){
      MaCI::Ranging::CRangingData *rdata = new MaCI::Ranging::CRangingData();
      rdata->CreateInternalBinBag();
      rdata->SetTimestamp(timestamp);
     
      rdata->SetDistanceHeader(MaCI::Ranging::TDistanceHeader(beam_width,beam_count));
      for(unsigned int i = 0; i< angles.size(); i++){
        rdata->AddDistance(MaCI::Ranging::TDistance(angles[i], distances[i]));

      }

      iData[timestamp.GetGimTime()] = rdata;
      iTimes.push_back(timestamp.GetGimTime());
    
    }
    

  }
  if(!first){
    iEnd = timestamp.GetGimTime();
  }

  return true;

}
//*****************************************************************************

bool RangingFilePlayer::OnTime(gim::time aTime)
{
  std::map<gim::time,MaCI::Ranging::CRangingData *>::iterator iter
    =iData.find(aTime);

  if(iter != iData.end()){
    iRangingServer->SendDistanceArrayEvent(*(iter->second));
    return true;
  }else{

    return false;
  }

}
//*****************************************************************************

bool RangingFilePlayer::GetTimes(std::list<gim::time> *aTimes)
{

  aTimes->assign(iTimes.begin(),iTimes.end());
  return true;
}
//*****************************************************************************

std::string RangingFilePlayer::GetName(void)
{
  return iName;

}
//*****************************************************************************

void RangingFilePlayer::GetTimeLimits(gim::time &aBegin,gim::time &aEnd)
{
  aBegin = iBegin;
  aEnd = iEnd;
  dPrint(15,"b %s, e %s", aBegin.toString().c_str(), aEnd.toString().c_str());
}
//*****************************************************************************



